#ifndef MOVE_YOUBOT_ARM_SERVER_H_
#define MOVE_YOUBOT_ARM_SERVER_H_

#include <iostream>
#include <assert.h>
#include <vector>

#include "ros/ros.h"

#include <sensor_msgs/JointState.h>
#include <actionlib/server/simple_action_server.h>

#include "youbot_relay/MoveYoubotArmAction.h"

namespace move_youbot_arm
{

/** A class that uses the actionlib::ActionServer interface that moves the youbot arm to a goal location.            */
class MoveYoubotArmServer
{
public:

  MoveYoubotArmServer(ros::NodeHandle n);

  virtual ~MoveYoubotArmServer();

private:

  static const int NUMBER_OF_ARM_JOINTS = 5;
  typedef boost::array<float, NUMBER_OF_ARM_JOINTS> ArmPosition;

  void execute(const youbot_relay::MoveYoubotArmGoalConstPtr& move_youbot_arm_goal);

  void publishArmPosition(const ArmPosition& position);

  void publishGripperPosition(float left, float right);

  void jointStateUpdate(const sensor_msgs::JointState& state);

  typedef actionlib::SimpleActionServer<youbot_relay::MoveYoubotArmAction> MoveYoubotArmActionServer;

  /// ROS node handle.
  ros::NodeHandle node_;

  /// Action server
  MoveYoubotArmActionServer* action_server_;

  /// Publisher of position commands to the youbot arm.
  ros::Publisher arm_position_publisher_;

  /// Publisher of position commands to the youbot arm gripper.
  ros::Publisher gripper_position_publisher_;

  ros::Subscriber joint_state_subscriber_;

};

}

#endif /* MOVE_YOUBOT_ARM_SERVER_H_ */

